
#define IN_RANGE             			1
#define OUT_OF_RANGE_NO_SOLUTION 		2
#define OUT_OF_RANGE         			3
#define NO_NEED_TO_MOVE					4

#define MATH_PI 			3.141592653589793238463
#define MATH_TRANS  		57.2958    
#define MATH_L1 		80.00//	107.45	
#define MATH_L2 		21.17//	21.17	
#define MATH_LOWER_ARM 		148.25	
#define MATH_UPPER_ARM 		160.2 	
#define MATH_FRONT_HEADER 	35.00//25.00// the distance between wrist to the front point we use
#define MATH_UPPER_LOWER 	MATH_UPPER_ARM/MATH_LOWER_ARM
#define MAX_Z				260		// max height
#define MIN_Z				(-120)

// 
#define RECTIFY(angle) (angle + 80.0)

unsigned char xyzToAngle(double x, double y, double z, double& angleRot, double& angleLeft, double& angleRight, boolean allowApproximate)
{
    double xIn = 0.0;
    double zIn = 0.0;
    double rightAll = 0.0;
    double sqrtZX = 0.0;
    double phi = 0.0;

    x = constrain(x,-3276,3276);
    y = constrain(y,-3276,3276);
    z = constrain(z,-3276,3276);
    x = (double)((int)(x*10)/10.0);
    y = (double)((int)(y*10)/10.0);
    z = (double)((int)(z*10)/10.0);


    if (z > MAX_Z || z < MIN_Z)
    {
        return OUT_OF_RANGE_NO_SOLUTION;
    }

    zIn = (z - MATH_L1) / MATH_LOWER_ARM;

    if(!allowApproximate)//if need the move to closest point we have to jump over the return function
    {
        //check the range of x
        if(y<0)
        {
            return OUT_OF_RANGE_NO_SOLUTION;
        }
    }
    // Calculate value of theta 1: the rotation angle
    if(x == 0)
    {
            angleRot = 90;
    }
    else
    {
            if (x > 0)
            {
                    angleRot = atan(y / x) * MATH_TRANS;//angle tranfer 0-180 CCW 弧度转为角度
            }
            if (x < 0)
            {
                    angleRot = 180 + atan(y / x) * MATH_TRANS;//angle tranfer  0-180 CCW
            }
    }
    // Calculate value of theta 3
    if(angleRot != 90)//xIn is the stretch
    {
            xIn = (x / cos(angleRot / MATH_TRANS) - MATH_L2 - MATH_FRONT_HEADER) / MATH_LOWER_ARM;
    }
    else
    {
            xIn = (y - MATH_L2 - MATH_FRONT_HEADER) / MATH_LOWER_ARM;
    }   

    phi = atan(zIn / xIn) * MATH_TRANS;//phi is the angle of line (from joint 2 to joint 4) with the horizon

    sqrtZX = sqrt(zIn*zIn + xIn*xIn); // 节点2到节点4的长度

    rightAll = (sqrtZX*sqrtZX + MATH_UPPER_LOWER * MATH_UPPER_LOWER  - 1) / (2 * MATH_UPPER_LOWER  * sqrtZX);//cosin law
    angleRight = acos(rightAll) * MATH_TRANS;//cosin law

    // Calculate value of theta 2
    rightAll = (sqrtZX*sqrtZX + 1 - MATH_UPPER_LOWER * MATH_UPPER_LOWER ) / (2 * sqrtZX);//cosin law
    angleLeft = acos(rightAll) * MATH_TRANS;//cosin law

    angleLeft = angleLeft + phi;
    angleRight = angleRight - phi;


    return 0;
    // //determine if the angle can be reached
    // return limitRange(angleRot, angleLeft, angleRight);
}
